/**
  ******************************************************************************
  * @file    pid_output.c
  * @author  Sherlock
  * @version V0.1
  * @date    08/12/2024
  * @brief   PID compute and output.
  ******************************************************************************
  * @copy
  *
  * <h2><center>&copy; COPYRIGHT 2010 Feirui</center></h2>
  */ 



/**
  * @brief  INCLUDE FILES.
  */
#ifndef	PID_OUTPUT_GLOBAL
#define	PID_OUTPUT_GLOBAL
#include <includes.h>
#include "pid_output.h"
#include "pid_driver.h"
#include "pid_timing.h"
#include "communi_print.h"


static uint8_t CheckIfExistShutOff(void);
static uint8_t CheckIfSynchronized(void);
static void CalAVRCrankPIDEnValue(NominalCfg_ST* TempNominalCfgBuffer, LoadEquConfig_ST* TempLoadCfgBuffer, float *TempVoltEn);
static void CalAVRNormalPIDEnValue(NominalCfg_ST* TempNominalCfgBuffer, LoadEquConfig_ST* TempLoadCfgBuffer, float *TempVoltEn);
static void CalAVRSyncPIDEnValue(NominalCfg_ST* TempNominalCfgBuffer, LoadEquConfig_ST* TempLoadCfgBuffer, float *TempVoltEn, float *TempStatorEn);
static void CalAVRPFPIDEnValue(NominalCfg_ST* TempNominalCfgBuffer, LoadEquConfig_ST* TempLoadCfgBuffer, float *TempVoltEn, float *TempStatorEn);
static uint8_t CombinPIDAdjOpeation(void);
static uint8_t TransferPIDResultToOutput(void);
static void InitExcitationPIDOutputObj(void);
static void InitStatorPIDOutputObj(void);
static void InitPIDAdjustTarget(void);


uint8_t PID_OutputHandle(void)
{
    uint8_t status = BenchWorkSignal_None;
/*****************************************************************************
 *                                事前检查
 *****************************************************************************/
    if(CheckIfExistShutOff())  //检查是否存在关断信号
    {
        status |= BenchWorkSignal_ShutOff;
    }
    if(!CheckIfSynchronized())
    {
        status |= BenchWorkSignal_OutOfSync;
    }

/*****************************************************************************
 *                                 工作模式
 *****************************************************************************/
    /* 获取PID相关配置 */
    NominalCfg_ST TempNominalCfgBuffer;
    PidDebugCfg_ST TempPIDCfgBuffer;
    LoadEquConfig_ST TempLoadCfgBuffer;
    DataBase_ReadDataContent(Addr_NominalCfg_RatedPhaseVolt, (uint16_t *)&TempNominalCfgBuffer, sizeof(TempNominalCfgBuffer)/2);
    DataBase_ReadDataContent(Addr_PidDebugCfg_ExciDivider_P, (uint16_t *)&TempPIDCfgBuffer, sizeof(TempPIDCfgBuffer)/2);
    DataBase_ReadDataContent(Addr_LoadEquCfg_LoadStartFreg, (uint16_t *)&TempLoadCfgBuffer, sizeof(TempLoadCfgBuffer)/2);

    /* 停机判定 */
    if(status & BenchWorkSignal_ShutOff)
    {
        testBenchWorkMode = BenchWorkMode_ShutOff;
        AVRHWOffDelayTiCnt = 1;
    }

    if(AVRStopDetectSta && testBenchWorkMode >= BenchWorkMode_Start)   //若检测到已经启动，则检测停机条件
    {
        if(ACAD_RTGlobalData.LoadVoltData.GenFreqAverage < TempLoadCfgBuffer.LoadStopFreg)   //发电频率小于18Hz，停机
        {
            testBenchWorkMode = BenchWorkMode_ShutOff;
            AVRHWOffDelayTiCnt = 1;
        }
    }

    switch (testBenchWorkMode)
    {
    case BenchWorkMode_Wait:
        if(ACAD_RTGlobalData.LoadVoltData.GenFreqAverage > TempLoadCfgBuffer.LoadStartFreg)  //大于15Hz时启动
        {
            AVRInitialTargetVolt = ACAD_RTGlobalData.LoadVoltData.GenFreqAverage;
            if(AVRInitialTargetVolt > TempNominalCfgBuffer.RatedPhaseVolt)   //如果初始电压就大于400V，则直接定在400V
                AVRInitialTargetVolt = TempNominalCfgBuffer.RatedPhaseVolt;
            if(TempLoadCfgBuffer.LoadSlowStartEnable)   //使能缓启动
            {
                AVRVoltCrankTiCnt = TempLoadCfgBuffer.LoadSlowStartDelay * PIDOperationTimingFreq;
            }
            else   //未使能，则使用阈值启动
            {
                AVRVoltCrankTiCnt = 0;
            }
            AVRVoltCrankFinish = 0;  //清空启动完成标志
            testBenchWorkMode = BenchWorkMode_Start;
        }
        break;
    
    case BenchWorkMode_ShutOff:
        InitPIDOutputObj();   //复位全部PID参数
        AVRStopDetectSta = DEF_FALSE;

        uint16_t tempHWShutOffDelay = TempLoadCfgBuffer.LoadHwshutOffDelay * PIDOperationTimingFreq;
        if(tempHWShutOffDelay == 0)   //如果设置关断时长为0，表示永不关断
            tempHWShutOffDelay = 65535;
        if(AVRHWOffDelayTiCnt > tempHWShutOffDelay)   //超过关断时长，则切换到静态
        {
            testBenchWorkMode = BenchWorkMode_Wait;
            AVRHWOffDelayTiCnt = 0;
        }
        break;
    
    case BenchWorkMode_Start:
        if(ACAD_RTGlobalData.LoadVoltData.GenFreqAverage > TempLoadCfgBuffer.LoadStopFreg)  //大于18Hz时，允许停机
            AVRStopDetectSta = DEF_TRUE;
        if(AVRVoltCrankFinish)  //启动完成
        {
            testBenchWorkMode=BenchWorkMode_AVR;
            // FillDataBaseDataContent(AdrSMGSta_ACActiveEnergy, 0, 4);   //启动后，将总电量数据清零
        }
    
    case BenchWorkMode_SyncAuto:
    case BenchWorkMode_SyncManual:
        if(!GPIO_ReadInputDataBit(PIDGPIO_SyncSwitchPort, PIDGPIO_SyncSwitchBitx))  //检测到软件合闸
        {
            if(GPIO_ReadInputDataBit(PIDGPIO_SyncStatusPort, PIDGPIO_SyncStatusBitx))  //读取到硬件是否同步，进入PF模式
            {
                testBenchWorkMode=BenchWorkMode_PF;
                pidSynchronousCnt=0;
                InitPIDAdjustTarget();
            }
        }
        break;

    case BenchWorkMode_PF:
        
    case BenchWorkMode_Async:
        if(!GPIO_ReadInputDataBit(PIDGPIO_SyncStatusPort, PIDGPIO_SyncStatusBitx))  //读取到当前已经失步，则进入AVR模式
        {
            testBenchWorkMode=BenchWorkMode_AVR;
            pidSynchronousCnt=0;
            InitStatorPIDOutputObj();   //分闸后复位定子
        }
        break;


    }
/*****************************************************************************
 *                                 误差计算
 *****************************************************************************/
    switch (testBenchWorkMode)
    {
    case BenchWorkMode_Wait:
        PID_SYNC_SW_OPEN;
        PIDExcitationOutput.Un = TempLoadCfgBuffer.LoadInitialPWM / 100;   //静态下固定5%输出
        
        break;

    case BenchWorkMode_ShutOff:
        PID_SYNC_SW_OPEN;

        break;

    case BenchWorkMode_Start:
        PID_SYNC_SW_OPEN;
        PIDExcitationOutput.Kp = PIDBaseValueExcitationKP*TempPIDCfgBuffer.VoltAdjust_P;
        PIDExcitationOutput.Ki = PIDBaseValueExcitationKI*TempPIDCfgBuffer.VoltAdjust_I;
        PIDExcitationOutput.Kd = PIDBaseValueExcitationKD*TempPIDCfgBuffer.VoltAdjust_D;
        // if(ACAD_RTGlobalData.LoadVoltData.GenFreqAverage > TempPIDCfgBuffer[AVRCfgOrd_StopFreq])  //大于18Hz时，允许停机
        //     AVRStopDetectSta = DEF_TRUE;
        CalAVRCrankPIDEnValue(&TempNominalCfgBuffer, &TempLoadCfgBuffer, &PIDExcitationOutput.ActiveErr);

        break;

    case BenchWorkMode_AVR:
        PID_SYNC_SW_OPEN;
        PIDExcitationOutput.Kp = PIDBaseValueExcitationKP*TempPIDCfgBuffer.VoltAdjust_P;
        PIDExcitationOutput.Ki = PIDBaseValueExcitationKI*TempPIDCfgBuffer.VoltAdjust_I;
        PIDExcitationOutput.Kd = PIDBaseValueExcitationKD*TempPIDCfgBuffer.VoltAdjust_D;
        CalAVRNormalPIDEnValue(&TempNominalCfgBuffer, &TempLoadCfgBuffer, &PIDExcitationOutput.ActiveErr);
        
        break;

    case BenchWorkMode_SyncAuto:
        PIDExcitationOutput.Kp = PIDBaseValueExcitationKP*TempPIDCfgBuffer.VoltAdjust_P;
        PIDExcitationOutput.Ki = PIDBaseValueExcitationKI*TempPIDCfgBuffer.VoltAdjust_I;
        PIDExcitationOutput.Kd = PIDBaseValueExcitationKD*TempPIDCfgBuffer.VoltAdjust_D;
        PIDStatorPIDOutput.Kp = PIDBaseValueStatorKP*TempPIDCfgBuffer.PhaseLockLoop_P;
        PIDStatorPIDOutput.Ki = PIDBaseValueStatorKI*TempPIDCfgBuffer.PhaseLockLoop_I;
        PIDStatorPIDOutput.Kd = PIDBaseValueStatorKD*TempPIDCfgBuffer.PhaseLockLoop_D;
        CalAVRSyncPIDEnValue(&TempNominalCfgBuffer, &TempLoadCfgBuffer, &PIDExcitationOutput.ActiveErr, &PIDStatorPIDOutput.ActiveErr);
        if(pidSynchronousCnt >= SyncSignalWaitTimesMin)   //检测到相位同步，可以进行合闸
        {
            PID_SYNC_SW_CLOSE;
        }
        else  //不符合同步调节，分闸
        {
            PID_SYNC_SW_OPEN;
        }
        break;
    
    case BenchWorkMode_SyncManual:
        PID_SYNC_SW_CLOSE;

        break;

    case BenchWorkMode_Async:
        PID_SYNC_SW_OPEN;

        break;

    case BenchWorkMode_PF:
        PIDExcitationOutput.Kp = PIDBaseValuePFExcitationKP*TempPIDCfgBuffer.ReacPower_P;
        PIDExcitationOutput.Ki = PIDBaseValuePFExcitationKI*TempPIDCfgBuffer.ReacPower_I;
        PIDExcitationOutput.Kd = PIDBaseValuePFExcitationKD*TempPIDCfgBuffer.ReacPower_D;
        PIDStatorPIDOutput.Kp = PIDBaseValueStatorKP*TempPIDCfgBuffer.ActivPower_P;
        PIDStatorPIDOutput.Ki = PIDBaseValueStatorKI*TempPIDCfgBuffer.ActivPower_I;
        PIDStatorPIDOutput.Kd = PIDBaseValueStatorKD*TempPIDCfgBuffer.ActivPower_D;
        CalAVRPFPIDEnValue(&TempNominalCfgBuffer, &TempLoadCfgBuffer, &PIDExcitationOutput.ActiveErr, &PIDStatorPIDOutput.ActiveErr);
        break;

    default:
        break;
    }


/*****************************************************************************
 *                                计算并输出
 *****************************************************************************/
    status = 0;  //清零状态
    if(testBenchWorkMode >=  BenchWorkMode_Start)   //只有启动后，才能计算PID输出，否则静态仅输出固定PWM
    {
        status |= CombinPIDAdjOpeation();  //计算PID结果
    }
    status |= TransferPIDResultToOutput();  //输出


    return DEF_TRUE;
}


static uint8_t CheckIfExistShutOff(void)
{

    return DEF_FALSE;
}


static uint8_t CheckIfSynchronized(void)
{

    return DEF_TRUE;
}


static void CalAVRCrankPIDEnValue(NominalCfg_ST* TempNominalCfgBuffer, LoadEquConfig_ST* TempLoadCfgBuffer, float *TempVoltEn)
{
	float TempCfgTargetVolt = TempNominalCfgBuffer->RatedPhaseVolt; //目标电压
    float TempRunTargetVolt, TempUFTargetVolt;

	//默认根据U/F特性启动，计算目标电压
	if(ACAD_RTGlobalData.LoadVoltData.GenFreqAverage >= TempLoadCfgBuffer->LoadLowFregTrip)  //当前频率大于转折频率，则目标电压可直接设为设定参考值
		TempUFTargetVolt = TempCfgTargetVolt;
	else
	{
        TempUFTargetVolt = TempCfgTargetVolt - (TempLoadCfgBuffer->LoadLowFregTrip - ACAD_RTGlobalData.LoadVoltData.GenFreqAverage) * TempLoadCfgBuffer->LoadLowFregRate;

		if(TempUFTargetVolt < 0)  //目标电压不能小于0，
			TempUFTargetVolt = 0;
	}

	TempRunTargetVolt = TempUFTargetVolt;
	if(TempLoadCfgBuffer->LoadSlowStartEnable) //使能了缓启动，则计算缓启动目标电压
	{
		float SlowStartTargetVolt;
		if(AVRInitialTargetVolt >= TempCfgTargetVolt)  //初始电压就超过了目标电压，则直接设缓启动电压为目标电压
		{
			SlowStartTargetVolt = TempCfgTargetVolt;
		}
		else  //根据缓启动时间设置缓启动电压
		{
			uint16_t TempCfgCrankTime = TempLoadCfgBuffer->LoadSlowStartDelay * PIDOperationTimingFreq;
			if(AVRVoltCrankTiCnt > TempCfgCrankTime)
				AVRVoltCrankTiCnt = TempCfgCrankTime;
			SlowStartTargetVolt = TempCfgTargetVolt - (float)AVRVoltCrankTiCnt * (TempCfgTargetVolt - AVRInitialTargetVolt) / (float)TempCfgCrankTime;
		}

		if(SlowStartTargetVolt < TempUFTargetVolt) //如果缓启动电压大于U/F特性电压，则设置U/F特性优先，否则缓启动优先
		{
			TempRunTargetVolt = SlowStartTargetVolt;
		}
	}

	if(TempRunTargetVolt >= TempCfgTargetVolt)  //如果目标电压已经大于参考，则结束启动阶段，进入正常工作模式
	  	AVRVoltCrankFinish=1;

	*TempVoltEn = TempRunTargetVolt != 0 ? (TempRunTargetVolt - ACAD_RTGlobalData.LoadVoltData.GenPhaseVoltAverage)/TempRunTargetVolt : 0;
	// *TempTSTEn=AVRRunTargetVolt != 0 ? SubstractionValue/AVRRunTargetVolt : 0;
}


static void CalAVRNormalPIDEnValue(NominalCfg_ST* TempNominalCfgBuffer, LoadEquConfig_ST* TempLoadCfgBuffer, float *TempVoltEn)
{
	float TempRunTargetVolt = TempNominalCfgBuffer->RatedPhaseVolt;  //目标电压

	*TempVoltEn = TempRunTargetVolt != 0 ? (TempRunTargetVolt - ACAD_RTGlobalData.LoadVoltData.GenPhaseVoltAverage) / TempRunTargetVolt : 0;
}


static void CalAVRSyncPIDEnValue(NominalCfg_ST* TempNominalCfgBuffer, LoadEquConfig_ST* TempLoadCfgBuffer, float *TempVoltEn, float *TempStatorEn)
{
	float VoltDiff = TempNominalCfgBuffer->RatedPhaseVolt != 0 ? ACAD_RTGlobalData.SyncData.VoltDiffAverage / TempNominalCfgBuffer->RatedPhaseVolt : 0;  //电压相对差异
    float FreqDiff = ACAD_RTGlobalData.GenVoltData.GenFreqAverage != 0 ? 
        (ACAD_RTGlobalData.GenVoltData.GenFreqAverage-ACAD_RTGlobalData.LoadVoltData.GenFreqAverage) / ACAD_RTGlobalData.GenVoltData.GenFreqAverage : 0;   //频率相对差异
    
    *TempVoltEn = VoltDiff;
    *TempStatorEn = ACAD_RTGlobalData.SyncData.SyncErrU1;
    if((VoltDiff < SyncLimit_MaxAlowVoltDiff && VoltDiff > -SyncLimit_MaxAlowVoltDiff) &&
        (FreqDiff < SyncLimit_MaxAlowFreqDiff && FreqDiff > -SyncLimit_MaxAlowFreqDiff) &&
        (ACAD_RTGlobalData.SyncData.PhaseDiffU1 < SyncLimit_MaxAlowPhaseDiff && ACAD_RTGlobalData.SyncData.PhaseDiffU1 > -SyncLimit_MaxAlowPhaseDiff))   //如果连续10次频率接近且相位差非常小，判定可以合闸
    {
        pidSynchronousCnt++;
        // *TempStatorEn = 0;
        // *TempVoltEn = 0;
        return;
    }
    if(pidSynchronousCnt)
        pidSynchronousCnt--;
}

static void CalAVRPFPIDEnValue(NominalCfg_ST* TempNominalCfgBuffer, LoadEquConfig_ST* TempLoadCfgBuffer, float *TempVoltEn, float *TempStatorEn)
{
    float setRatedPower = TempNominalCfgBuffer->RatedPhaseVolt*TempNominalCfgBuffer->RatedCurrent*3/1000;  //用系统总额定功率来作为分母
    setRatedPower = setRatedPower > 1 ? setRatedPower : 1;
    
    float setActivePower = PIDAdjustTarget[PIDAdjustTarget_ActivePower];
    float setPF = PIDAdjustTarget[PIDAdjustTarget_PowerFactor];
    if(setPF <= (float)0.1)
        setPF = 0.1;
    float setApparentPower = setActivePower / setPF;
    //float refApparentPower = setApparentPower > rtApparentPower ? setApparentPower : rtApparentPower;
    /* 转矩输出越高，有功功率越大，功率因数越大 */
    setApparentPower = setApparentPower > 1 ? setApparentPower : 1;
    *TempStatorEn = (setActivePower - ACAD_RTGlobalData.PowerData.GenPowerP1/1000)/setRatedPower;
    
    /* 励磁输出越高，功率因数越小 */
    // float setRePF = sqrt(1-setPF*setPF);
    // float rtRePF = ACAD_RTGlobalData.PowerData.GenPowerS1 != 0 ? (ACAD_RTGlobalData.PowerData.GenPowerQ1 / ACAD_RTGlobalData.PowerData.GenPowerS1) : setRePF;
    // *TempVoltEn = (setRePF - rtRePF)/2;
    /* 励磁输出越高，无功功率越大 */
    float setRePF = sqrt(1-setPF*setPF);
    float setReactivePower = setApparentPower * setRePF;
    *TempVoltEn = (setReactivePower - ACAD_RTGlobalData.PowerData.GenPowerQ1/1000)/setRatedPower;

}


static uint8_t CombinPIDAdjOpeation(void)
{
    /* 停机状态下不允许输出 */
    if(testBenchWorkMode == BenchWorkMode_Wait || testBenchWorkMode == BenchWorkMode_ShutOff)
    {
        InitPIDOutputObj();
        return DEF_TRUE;
    }

    //CalTemp0(比例)，CalTemp1(积分)，CalTemp2(微分总量)，CalTemp3(一般微分分量)，CalTemp4(瞬态微分分量(如有需要))
    float CalTemp0,CalTemp1,CalTemp2,CalTemp3=0,CalTemp4=0,CalTemp5;
/*******************************************************************************
                                计算定子旋转方向
********************************************************************************/
/* 限制幅度，防止异常数字进入计算，导致的异常溢出 */
	if(PIDStatorPIDOutput.ActiveErr > 1) PIDStatorPIDOutput.ActiveErr = 1;
	if(PIDStatorPIDOutput.ActiveErr < -1) PIDStatorPIDOutput.ActiveErr = -1;
	if(PIDStatorPIDOutput.TSTErr > 1) PIDStatorPIDOutput.TSTErr = 1;
	if(PIDStatorPIDOutput.TSTErr < -1) PIDStatorPIDOutput.TSTErr = -1;

    /* @Up=Kp*Gp*E(n) */
	CalTemp0=PIDStatorPIDOutput.ActiveErr;	
	/********/
	/* @Ui=Ui+Ki*Gi*E(n) */
	CalTemp1=PIDStatorPIDOutput.ActiveErr;

#ifdef PIDFullDiffOpt
  	/* 完全微分 */
	/* @Ud=a*Kd*[E(n)-E(n-1)]+(1-a)*Ud(n-1) */
	CalTemp3=PIDStatorPIDOutput.ActiveErr-PIDStatorPIDOutput.LastErr;
#endif

#ifdef PIDTSTErrDiffOpt
	//瞬态误差作为微分输入，这里有一个负号，是因为要抑制这种误差趋势，而不能放大误差
	CalTemp4=-PIDStatorPIDOutput.TSTErr*PIDTSTDiffPartial;
	CalTemp3*=PIDTraDiffPartial;
#else
	/* 无瞬态微分 */
	CalTemp4=0;
#endif

    /* 变速PID增益 */
#ifdef PIDShiftSpeedOpt
    /* 变比例 */
    if(CalTemp0 > 0)
    {
        CalTemp0 = (1+CalTemp0)*(1+CalTemp0)*(1+CalTemp0)*(1+CalTemp0) - 1;
    }
    else
    {
        CalTemp0 = 1 - (1-CalTemp0)*(1-CalTemp0)*(1-CalTemp0)*(1-CalTemp0);
    }

    /* 变积分 */
    if(CalTemp1 > 0)
    {
        CalTemp1 = CalTemp1*(1-CalTemp1)*(1-CalTemp1)*(1-CalTemp1);
    }
    else
    {
        CalTemp1 = CalTemp1*(1+CalTemp1)*(1+CalTemp1)*(1+CalTemp1);
    }

    /* 变微分 */
	CalTemp2=CalTemp3+CalTemp4;  //两路微分汇总
    if(CalTemp2 > 0)
    {
        CalTemp2 = (1+CalTemp2)*(1+CalTemp2)*(1+CalTemp2) - 1;
    }
    else
    {
        CalTemp2 = 1 - (1-CalTemp2)*(1-CalTemp2)*(1-CalTemp2);
    }

	CalTemp0*=PIDStatorPIDOutput.Kp;
	CalTemp1*=PIDStatorPIDOutput.Ki;
	CalTemp2*=PIDStatorPIDOutput.Kd;

#else
	CalTemp0*=PIDStatorPIDOutput.Kp;
	CalTemp1*=PIDStatorPIDOutput.Ki;
	CalTemp2=(CalTemp3+CalTemp4)*PIDStatorPIDOutput.Kd;  //两路微分汇总
#endif

    /* 微分滤波 */
	float KDafactorPhaseBase = (1-PIDStatorPIDOutput.Kdf) / KDaFactorTotal;
	CalTemp2 = CalTemp2*PIDStatorPIDOutput.Kdf + KDafactorPhaseBase*(PIDStatorPIDOutput.Ud2*KDaFactor2+PIDStatorPIDOutput.Ud3*KDaFactor3+PIDStatorPIDOutput.Ud4*KDaFactor4+PIDStatorPIDOutput.Ud5*KDaFactor5+PIDStatorPIDOutput.Ud6*KDaFactor6+PIDStatorPIDOutput.Ud7*KDaFactor7+PIDStatorPIDOutput.Ud8*KDaFactor8+PIDStatorPIDOutput.Ud9*KDaFactor9+PIDStatorPIDOutput.Ud10*KDaFactor10);
	// CalTemp2 = CalTemp4;
	PIDStatorPIDOutput.Ud += CalTemp2;   //微分累计
	if(PIDStatorPIDOutput.Ud > InputDiffMaxAccumulation)
	{
		PIDStatorPIDOutput.Ud = InputDiffMaxAccumulation;
	}
	else if(PIDStatorPIDOutput.Ud < -InputDiffMaxAccumulation*0.5)
	{
		PIDStatorPIDOutput.Ud = -InputDiffMaxAccumulation*0.5;
	}
	/* 将往次结果向后移位 */
	PIDStatorPIDOutput.Ud10 = PIDStatorPIDOutput.Ud9;
	PIDStatorPIDOutput.Ud9 = PIDStatorPIDOutput.Ud8;
	PIDStatorPIDOutput.Ud8 = PIDStatorPIDOutput.Ud7;
	PIDStatorPIDOutput.Ud7 = PIDStatorPIDOutput.Ud6;
	PIDStatorPIDOutput.Ud6 = PIDStatorPIDOutput.Ud5;
	PIDStatorPIDOutput.Ud5 = PIDStatorPIDOutput.Ud4;
	PIDStatorPIDOutput.Ud4 = PIDStatorPIDOutput.Ud3;
	PIDStatorPIDOutput.Ud3 = PIDStatorPIDOutput.Ud2;
	PIDStatorPIDOutput.Ud2 = CalTemp2;

	/* 积分饱和法，如果上一次的积分加当次的比例输出已经饱和，且本次积分的方向依然保持饱和，则本次不再累计积分 */
	CalTemp3 = CalTemp0 + PIDStatorPIDOutput.Ui;
	if(CalTemp3>=1)
	{
		if(CalTemp1>0)
			CalTemp1=0;
	}
	else if(CalTemp3<=0)
	{
		if(CalTemp1<0)
			CalTemp1=0;
	}
    CalTemp1+=PIDStatorPIDOutput.Ui;
	PIDStatorPIDOutput.Ui=CalTemp1; //更新积分累计

	/* 按需将微分带入总输出 */
    CalTemp3 = CalTemp0 + CalTemp1; //先计算比例、积分之和，再根据情况带入微分分量
	if(CalTemp3>=OutputDiffMaxAccumulation && PIDStatorPIDOutput.Ud>=0)
	{

	}
	else if(CalTemp3<=0 && PIDStatorPIDOutput.Ud<=0)
	{

	}
	else
	{
		if((CalTemp3+PIDStatorPIDOutput.Ud)>=OutputDiffMaxAccumulation)
		{
			PIDStatorPIDOutput.Ud+=(CalTemp3-OutputDiffMaxAccumulation);
			CalTemp3=OutputDiffMaxAccumulation;
		}
		else if((CalTemp3+PIDStatorPIDOutput.Ud)<=0)
		{
			PIDStatorPIDOutput.Ud+=CalTemp3;
			CalTemp3=0;
		}
		else
		{
			CalTemp3+=PIDStatorPIDOutput.Ud;
			PIDStatorPIDOutput.Ud=0;
		}
	}

	PIDStatorPIDOutput.LastErr=PIDStatorPIDOutput.ActiveErr;	 //E(n-1)<--E(n)
	CalTemp5=CalTemp3*PIDStatorPIDOutput.LoopGain;

	PIDStatorPIDOutput.Un=CalTemp5;  //输出结果给到Un

	if(CommPrintDataNum)
  	{
		CommData_PrintTotalQueue[CommData_PIDStatorOutputP] = CalTemp0;
		CommData_PrintTotalQueue[CommData_PIDStatorOutputI] = CalTemp1;
		CommData_PrintTotalQueue[CommData_PIDStatorOutputD] = CalTemp2;
		CommData_PrintTotalQueue[CommData_PIDStatorOutput] = CalTemp5;
	}
#if 0
    if(PIDStatorOutput.ActiveErr > PIDStatorOutput.threshold)
    {
        PIDStatorOutput.statorOutput = StatorOutput_Up;
    }
    else if(PIDStatorOutput.ActiveErr < -PIDStatorOutput.threshold)
    {
        PIDStatorOutput.statorOutput = StatorOutput_Down;
    }
    else
    {
        PIDStatorOutput.statorOutput = StatorOutput_Stay;
    }
#endif

/*******************************************************************************
                                计算励磁输出PWM大小
********************************************************************************/
    /* 限制幅度，防止异常数字进入计算，导致的异常溢出 */
	if(PIDExcitationOutput.ActiveErr > 1) PIDExcitationOutput.ActiveErr = 1;
	if(PIDExcitationOutput.ActiveErr < -1) PIDExcitationOutput.ActiveErr = -1;
	if(PIDExcitationOutput.TSTErr > 1) PIDExcitationOutput.TSTErr = 1;
	if(PIDExcitationOutput.TSTErr < -1) PIDExcitationOutput.TSTErr = -1;

    /* @Up=Kp*Gp*E(n) */
	CalTemp0=PIDExcitationOutput.ActiveErr;	
	/********/
	/* @Ui=Ui+Ki*Gi*E(n) */
	CalTemp1=PIDExcitationOutput.ActiveErr;

#ifdef PIDFullDiffOpt
  	/* 完全微分 */
	/* @Ud=a*Kd*[E(n)-E(n-1)]+(1-a)*Ud(n-1) */
	CalTemp3=PIDExcitationOutput.ActiveErr-PIDExcitationOutput.LastErr;
#endif

#ifdef PIDTSTErrDiffOpt
	//瞬态误差作为微分输入，这里有一个负号，是因为要抑制这种误差趋势，而不能放大误差
	CalTemp4=-PIDExcitationOutput.TSTErr*PIDTSTDiffPartial;
	CalTemp3*=PIDTraDiffPartial;
#else
	/* 无瞬态微分 */
	CalTemp4=0;
#endif

    /* 变速PID增益 */
#ifdef PIDShiftSpeedOpt
    /* 变比例 */
    if(CalTemp0 > 0)
    {
        CalTemp0 = (1+CalTemp0)*(1+CalTemp0)*(1+CalTemp0)*(1+CalTemp0) - 1;
    }
    else
    {
        CalTemp0 = 1 - (1-CalTemp0)*(1-CalTemp0)*(1-CalTemp0)*(1-CalTemp0);
    }

    /* 变积分 */
    if(CalTemp1 > 0)
    {
        CalTemp1 = CalTemp1*(1-CalTemp1)*(1-CalTemp1)*(1-CalTemp1);
    }
    else
    {
        CalTemp1 = CalTemp1*(1+CalTemp1)*(1+CalTemp1)*(1+CalTemp1);
    }

    /* 变微分 */
	CalTemp2=CalTemp3+CalTemp4;  //两路微分汇总
    if(CalTemp2 > 0)
    {
        CalTemp2 = (1+CalTemp2)*(1+CalTemp2)*(1+CalTemp2) - 1;
    }
    else
    {
        CalTemp2 = 1 - (1-CalTemp2)*(1-CalTemp2)*(1-CalTemp2);
    }

	CalTemp0*=PIDExcitationOutput.Kp;
	CalTemp1*=PIDExcitationOutput.Ki;
	CalTemp2*=PIDExcitationOutput.Kd;

#else
	CalTemp0*=PIDExcitationOutput.Kp;
	CalTemp1*=PIDExcitationOutput.Ki;
	CalTemp2=(CalTemp3+CalTemp4)*PIDExcitationOutput.Kd;  //两路微分汇总
#endif

    /* 微分滤波 */
	float KDafactorBase = (1-PIDExcitationOutput.Kdf) / KDaFactorTotal;
	CalTemp2 = CalTemp2*PIDExcitationOutput.Kdf + KDafactorBase*(PIDExcitationOutput.Ud2*KDaFactor2+PIDExcitationOutput.Ud3*KDaFactor3+PIDExcitationOutput.Ud4*KDaFactor4+PIDExcitationOutput.Ud5*KDaFactor5+PIDExcitationOutput.Ud6*KDaFactor6+PIDExcitationOutput.Ud7*KDaFactor7+PIDExcitationOutput.Ud8*KDaFactor8+PIDExcitationOutput.Ud9*KDaFactor9+PIDExcitationOutput.Ud10*KDaFactor10);
	// CalTemp2 = CalTemp4;
	PIDExcitationOutput.Ud += CalTemp2;   //微分累计
	if(PIDExcitationOutput.Ud > InputDiffMaxAccumulation)
	{
		PIDExcitationOutput.Ud = InputDiffMaxAccumulation;
	}
	else if(PIDExcitationOutput.Ud < -InputDiffMaxAccumulation*0.5)
	{
		PIDExcitationOutput.Ud = -InputDiffMaxAccumulation*0.5;
	}
	/* 将往次结果向后移位 */
	PIDExcitationOutput.Ud10 = PIDExcitationOutput.Ud9;
	PIDExcitationOutput.Ud9 = PIDExcitationOutput.Ud8;
	PIDExcitationOutput.Ud8 = PIDExcitationOutput.Ud7;
	PIDExcitationOutput.Ud7 = PIDExcitationOutput.Ud6;
	PIDExcitationOutput.Ud6 = PIDExcitationOutput.Ud5;
	PIDExcitationOutput.Ud5 = PIDExcitationOutput.Ud4;
	PIDExcitationOutput.Ud4 = PIDExcitationOutput.Ud3;
	PIDExcitationOutput.Ud3 = PIDExcitationOutput.Ud2;
	PIDExcitationOutput.Ud2 = CalTemp2;

	/* 积分饱和法，如果上一次的积分加当次的比例输出已经饱和，且本次积分的方向依然保持饱和，则本次不再累计积分 */
	CalTemp3 = CalTemp0 + PIDExcitationOutput.Ui;
	if(CalTemp3>=1)
	{
		if(CalTemp1>0)
			CalTemp1=0;
	}
	else if(CalTemp3<=0)
	{
		if(CalTemp1<0)
			CalTemp1=0;
	}
    CalTemp1+=PIDExcitationOutput.Ui;
	PIDExcitationOutput.Ui=CalTemp1; //更新积分累计

	/* 按需将微分带入总输出 */
    CalTemp3 = CalTemp0 + CalTemp1; //先计算比例、积分之和，再根据情况带入微分分量
	if(CalTemp3>=OutputDiffMaxAccumulation && PIDExcitationOutput.Ud>=0)
	{

	}
	else if(CalTemp3<=0 && PIDExcitationOutput.Ud<=0)
	{

	}
	else
	{
		if((CalTemp3+PIDExcitationOutput.Ud)>=OutputDiffMaxAccumulation)
		{
			PIDExcitationOutput.Ud+=(CalTemp3-OutputDiffMaxAccumulation);
			CalTemp3=OutputDiffMaxAccumulation;
		}
		else if((CalTemp3+PIDExcitationOutput.Ud)<=0)
		{
			PIDExcitationOutput.Ud+=CalTemp3;
			CalTemp3=0;
		}
		else
		{
			CalTemp3+=PIDExcitationOutput.Ud;
			PIDExcitationOutput.Ud=0;
		}
	}

	PIDExcitationOutput.LastErr=PIDExcitationOutput.ActiveErr;	 //E(n-1)<--E(n)
	CalTemp5=CalTemp3*PIDExcitationOutput.LoopGain;

	PIDExcitationOutput.Un=CalTemp5;  //输出结果给到Un

	if(CommPrintDataNum)
  	{
		CommData_PrintTotalQueue[CommData_PIDExciteOutputP] = CalTemp0;
		CommData_PrintTotalQueue[CommData_PIDExciteOutputI] = CalTemp1;
		CommData_PrintTotalQueue[CommData_PIDExciteOutputD] = CalTemp2;
		CommData_PrintTotalQueue[CommData_PIDExciteOutput] = CalTemp5;
	}

    return DEF_TRUE;
}


static uint8_t TransferPIDResultToOutput(void)
{
/*******************************************************************************
                                定子旋转
********************************************************************************/
#if 0
    switch (PIDStatorOutput.statorOutput)
    {
    case StatorOutput_Up:
        GPIO_SetBits(PIDGPIO_StatorOut1Port, PIDGPIO_StatorOut1Bitx);
        GPIO_SetBits(PIDGPIO_StatorOut2Port, PIDGPIO_StatorOut2Bitx);
        break;
    case StatorOutput_Down:
        GPIO_SetBits(PIDGPIO_StatorOut1Port, PIDGPIO_StatorOut1Bitx);
        GPIO_ResetBits(PIDGPIO_StatorOut2Port, PIDGPIO_StatorOut2Bitx);
        break;
    default:
        GPIO_ResetBits(PIDGPIO_StatorOut1Port, PIDGPIO_StatorOut1Bitx);
        GPIO_ResetBits(PIDGPIO_StatorOut2Port, PIDGPIO_StatorOut2Bitx);
        break;
    }
#endif
/*******************************************************************************
                                励磁输出PWM
********************************************************************************/
    float PID_Un = PIDExcitationOutput.Un;
    if(PID_Un<0) PID_Un=0;
	else if(PID_Un>1) PID_Un=1;
	
	PID_Un*=PIDTIM_Period;
	PID_Un=PIDTIM_Period-PID_Un; //高端产品取消反向
  
	if(PID_Un<1)
		PID_Un=1;
	else if(PID_Un>(PIDTIM_Period-1))
  	PID_Un=PIDTIM_Period-1;
    
	TIM_SetCmp1(PIDTIM, (uint16_t)PID_Un);

/*******************************************************************************
                                锁相环PWM
********************************************************************************/
    PID_Un = PIDStatorPIDOutput.Un;
    if(PID_Un<0) PID_Un=0;
	else if(PID_Un>1) PID_Un=1;
	// PID_Un = 1-PID_Un;  //反向

    float tempOutputCenter;
    DataBase_ReadDataContent(Addr_LoadEquCfg_GovMiddleValue, (uint16_t *)&tempOutputCenter, 2);
    tempOutputCenter /= 100;   //校准中心点，临时做法
    float tempOutputMin, tempOutputMax;
    if(tempOutputCenter > 0.5)
    {
        tempOutputMin = 2*tempOutputCenter - 1;
        tempOutputMax = 1;
    }
    else
    {
        tempOutputMin = 0;
        tempOutputMax = 2*tempOutputCenter;
    }
    PID_Un = (tempOutputMax-tempOutputMin)*PID_Un + tempOutputMin;
	PID_Un*=PIDTIM4_Period;
	// PID_Un=PIDTIM4_Period-PID_Un; //高端产品取消反向
  
	TIM_SetCmp4(PIDTIM4, (uint16_t)PID_Un);  //临时输出50%PWM到PB9

    return DEF_TRUE;
}


static void InitExcitationPIDOutputObj(void)
{
    PIDExcitationOutput.Un = 0;   //关断励磁输出
    PIDExcitationOutput.Ui = 0;
    PIDExcitationOutput.ActiveErr = 0;
    PIDExcitationOutput.LastErr = 0;
    PIDExcitationOutput.TSTErr = 0;
    PIDExcitationOutput.Ud=0;    
	PIDExcitationOutput.Ud2=0;
	PIDExcitationOutput.Ud3=0;
	PIDExcitationOutput.Ud4=0;
	PIDExcitationOutput.Ud5=0;
	PIDExcitationOutput.Ud6=0;
	PIDExcitationOutput.Ud7=0;
	PIDExcitationOutput.Ud8=0;
	PIDExcitationOutput.Ud9=0;  
	PIDExcitationOutput.Ud10=0;
}


static void InitStatorPIDOutputObj(void)
{
    PIDStatorPIDOutput.Un = 0.5;  //保持中心点
    PIDStatorPIDOutput.Ui = 0.5;  //保持中心点
    PIDStatorPIDOutput.ActiveErr = 0;
    PIDStatorPIDOutput.LastErr = 0;
    PIDStatorPIDOutput.TSTErr = 0;
    PIDStatorPIDOutput.Ud=0;    
	PIDStatorPIDOutput.Ud2=0;
	PIDStatorPIDOutput.Ud3=0;
	PIDStatorPIDOutput.Ud4=0;
	PIDStatorPIDOutput.Ud5=0;
	PIDStatorPIDOutput.Ud6=0;
	PIDStatorPIDOutput.Ud7=0;
	PIDStatorPIDOutput.Ud8=0;
	PIDStatorPIDOutput.Ud9=0;  
	PIDStatorPIDOutput.Ud10=0;

    PIDStatorOutput.statorOutput = StatorOutput_Stay;  //关断定子旋转输出

    /* 预装初始有功功率和功率因数，下次合闸后可以使用 */
    InitPIDAdjustTarget();
}


void InitPIDOutputObj(void)
{
    InitExcitationPIDOutputObj();
    InitStatorPIDOutputObj();
}


static void InitPIDAdjustTarget(void)
{
    float tempPFBuffer;
    DataBase_ReadDataContent(Addr_NominalCfg_RatedPowerFactor, (uint16_t *)&tempPFBuffer, 2);
    PIDAdjustTarget[PIDAdjustTarget_ActivePower] = 0;
    PIDAdjustTarget[PIDAdjustTarget_PowerFactor] = tempPFBuffer;
}


#endif	/*	PID_OUTPUT_GLOBAL	*/

/******************* (C) COPYRIGHT 2010 Feirui *****END OF FILE****/